
// 读取adc的数据  使用udock读取json数据

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "basic_sprayer_interfaces/msg/adc.hpp"       //adc的Message
#include "basic_sprayer_interfaces/msg/human_cmd.hpp" // 控制信号

#include <QCoreApplication>
#include <QTimer>
#include <QSerialPort>
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>
#include <QDebug>

#include "crsf_protocol.h"

using namespace std::chrono_literals;

class Crc8
{
public:
    Crc8(uint8_t poly);
    uint8_t calc(uint8_t *data, uint8_t len);

protected:
    uint8_t _lut[256];
    void init(uint8_t poly);
};

Crc8::Crc8(uint8_t poly)
{
    init(poly);
}

void Crc8::init(uint8_t poly)
{
    for (int idx = 0; idx < 256; ++idx)
    {
        uint8_t crc = idx;
        for (int shift = 0; shift < 8; ++shift)
        {
            crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0);
        }
        _lut[idx] = crc & 0xff;
    }
}

uint8_t Crc8::calc(uint8_t *data, uint8_t len)
{
    uint8_t crc = 0;
    while (len--)
    {
        crc = _lut[crc ^ *data++];
    }
    return crc;
}

/* 接收来自某个串口的数据 通过参数  */
class AdcJson : public QObject
{
public:
    rclcpp::Node::SharedPtr nh_;
    rclcpp::Publisher<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr publisher_;
    rclcpp::Subscription<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr subscription_nav_cmd;
    rclcpp::TimerBase::SharedPtr timer_10ms;

    // rclcpp::PublisherOptions publisher_options_;
    explicit AdcJson(rclcpp::Node::SharedPtr &node, QObject *parent = nullptr) : QObject(parent)
    {
        nh_ = node;
        /* 指明参数 */
        rcl_interfaces::msg::ParameterDescriptor serial_descriptor;
        serial_descriptor.description = "which tty device to connect for adc";
        rclcpp::ParameterValue p_serial = nh_->declare_parameter("serial", rclcpp::ParameterValue("/dev/crsf_serial"), serial_descriptor);
        // prclcpp::ParameterValue p_serial = nh_->declare_parameter("serial", rclcpp::ParameterValue("/dev/ttyUSB0"), serial_descriptor);

        crc = new Crc8(0xd5);
        // publisher_ = nh_->create_publisher<basic_sprayer_interfaces::msg::Adc>("/hesiwei/adc", 10);
        publisher_ = nh_->create_publisher<basic_sprayer_interfaces::msg::HumanCmd>("/hesiwei/humancmd", 10);

        /*订阅导航的命令*/
        subscription_nav_cmd = nh_->create_subscription<basic_sprayer_interfaces::msg::HumanCmd>(
            "/nav_cmd", 10, std::bind(&AdcJson::callback_nav_cmd, this, std::placeholders::_1));

        timer_10ms = nh_->create_wall_timer(10ms, std::bind(&AdcJson::timer_10ms_callback, this));

        serial_port = new QSerialPort();
        std::string str;
        nh_->get_parameter("serial", str);
        RCLCPP_INFO(nh_->get_logger(), "connect tty:" + str);
        serial_port->setPortName(QString::fromStdString(str));
        serial_port->setBaudRate(420000, QSerialPort::AllDirections); // 设置波特率和读写方向   416666
        serial_port->setDataBits(QSerialPort::Data8);                 // 数据位为8位
        serial_port->setFlowControl(QSerialPort::NoFlowControl);      // 无流控制
        serial_port->setParity(QSerialPort::NoParity);                // 无校验位
        serial_port->setStopBits(QSerialPort::OneStop);               // 一位停止位
        serial_port->close();

        // 连接信号槽 当下位机发送数据QSerialPortInfo 会发送个 readyRead 信号,我们定义个槽void receiveInfo()解析数据
        connect(serial_port, &QSerialPort::readyRead, this, &AdcJson::readyRead);
        connect(serial_port, &QSerialPort::errorOccurred, this, &AdcJson::errorOccurred);

        /* 立即启动 */
        // timer = new QTimer(this);
        // connect(timer, &QTimer::timeout, this, &AdcJson::timeout);
        // timer->start(1000);
        timer_id_1s = startTimer(1000);
        // timer_id_10ms = startTimer(10);
        memset(&linkStatistics, 0, sizeof(linkStatistics));
    }

    // 接收导航 的命令
    void callback_nav_cmd(const basic_sprayer_interfaces::msg::HumanCmd::SharedPtr msg)
    {
        if (man_nav_mode == 1)
        {
            /* 发布导航的消息 */
            auto message = *msg;
            publisher_->publish(message);
        }
        else if (man_nav_mode == 2)
        {
            auto message = basic_sprayer_interfaces::msg::HumanCmd();
            message.header.stamp = nh_->now();
            /* 停下车子 */
            message.cmd_switch = message.CMD_STOP;
            message.velocity = 0;
            message.angle = 0;
            /* 发布数据 */
            publisher_->publish(message);
        }
    }
#define ANGLE channels[0]    // 转角
#define THROTTLE channels[2] // 油门
#define LOCK channels[4]     // 启动或者是停止
#define DIR channels[5]      // 方向
#define MAN channels[6]      // 手动    大于0 手动模式
    /* 判断 crsf 工作是否ok 运行频率 100Hz */
    bool is_crsf_ok()
    {
        /* 判断有多长时间没有 接收到数据 */
        if (flag_rc)
        {
            flag_rc = false;
            flag_rc_cnt = 0;
        }
        else
        {
            if (flag_rc_cnt > 10)
            {
                rc_connect = false;
            }
            else
            {
                flag_rc_cnt++;
            }
        }
        // if (linkStatistics.uplink_Link_quality <= 20)
        // {
        //     RCLCPP_INFO(nh_->get_logger(), "low quality! lock.");
        // }
        /* 必须要 rc连接正常  信号强度正常  usb连接正常  才认为是正常的 */
        return rc_connect && (linkStatistics.uplink_Link_quality > 20) && usb_connect;
    }
    /* 遥控器发布控制信号 */
    void timer_10ms_callback()
    {
        /*
            遥控数据接收：
                接收数据：启动标志，转向角，速度；
        */
        /*
             新设置 -> 通道配置：
                 CH[0] = Ail  --> ailereon 的缩写，为副翼，控制无人机绕纵轴做横滚运动（Roll）
                 CH[1] = Ele  --> elevator 的缩写，为升降舵，俯仰运动（Pitch）
                 CH[2] = Thr  --> throttle 的缩写，为油门，控制无人机沿立轴上升或下降
                 CH[3] = Rud  --> rudder   的缩写，为方向舵，控制无人机绕立轴做偏航运动（Yaw）
                 CH[4] = start_stop_lock   0或者1
                 CH[5] = dir   --> 设置方向  -> -1  or  1
        */
        if (is_crsf_ok())
        { /* 连接正常 */
            if (man_nav_mode != 0)
            {
                /* 人工控制 */
                if (man_nav_mode == 1)
                {
                    if (MAN > joy_high_value)
                    {
                        man_nav_mode = 0;
                        RCLCPP_INFO(nh_->get_logger(), "manual mode.");
                    }
                }
                else
                { // man_nav_mode == 2
                    if (MAN > joy_high_value)
                    {
                        man_nav_mode = 0;
                        RCLCPP_INFO(nh_->get_logger(), "navigation unlock to manual mode.");
                    }
                }
            }
            else if (joy_lock == true)
            {
                if ((LOCK < joy_mid_value) && (THROTTLE < joy_low_value))
                {
                    joy_lock = false;
                    RCLCPP_INFO(nh_->get_logger(), "ok, unlock.");
                }
            }
            else if (joy_start == false)
            {
                if (LOCK < joy_mid_value)
                {
                    joy_stop_cnt = 0;
                }
                else if ((LOCK > joy_mid_value) && (THROTTLE < joy_low_value) && (joy_stop_cnt == 0))
                {
                    joy_start = true;
                    if (DIR < 500)
                    {
                        joy_dir = -1;
                    }
                    else if (DIR > 1700)
                    {
                        joy_dir = 1;
                    }
                    else
                    {
                        joy_start = false;
                    }
                    /* 遥控信号正常的时候 才会作此判断 */
                }
                /* 设置为 */
                if (MAN < joy_low_value)
                {
                    man_nav_mode = 1;
                    RCLCPP_INFO(nh_->get_logger(), "navigation mode.");
                }
            }
            else
            {
                if (LOCK < joy_mid_value)
                {
                    joy_start = false;
                    RCLCPP_INFO(nh_->get_logger(), "active lock.");
                }
                if (THROTTLE < joy_low_value)
                {
                    joy_stop_cnt++;
                    if (joy_stop_cnt >= joy_stop_max)
                    {
                        joy_start = false;
                        joy_lock = true;
                        joy_stop_cnt = joy_stop_max;
                    }
                }
                else
                {
                    joy_stop_cnt = 0;
                }
            }
        }
        else
        {
            /* 连接有问题 */
            joy_lock = true; // 锁起来
            joy_stop_cnt = 0;
            joy_start = false;
            if (man_nav_mode == 1)
            {
                man_nav_mode = 2;
                RCLCPP_INFO(nh_->get_logger(), "navigation lock.");
            }
        }
        if (man_nav_mode == 0)
        {
            auto message = basic_sprayer_interfaces::msg::HumanCmd();
            message.header.stamp = nh_->now();
            if ((joy_lock == false) && joy_start)
            {
                message.cmd_switch = message.CMD_START;
                float speed = THROTTLE - joy_low_value;

                message.velocity = speed > 0 ? joy_dir * speed * 2.f / 1500 : 0;
                message.angle = -(ANGLE - 992) * (24.f / 818);
            }
            else
            {
                message.cmd_switch = message.CMD_STOP;
                message.velocity = 0;
                message.angle = -(ANGLE - 992) * (24.f / 818);
            }
            /* 发布数据 */
            publisher_->publish(message);
        }
    }

    void spin()
    {
        rclcpp::spin(nh_);
    }
    /* 开一个新的线程 并且等待结束 */
    void new_thread_spin()
    {
        std::thread *spin = new std::thread(std::bind(&AdcJson::spin, this)); // 不删除了  已经结束了
        // std::thread spin(std::bind(&AdcJson::spin, this));
        spin->detach();
    }

    ~AdcJson()
    {
        serial_port->close();
    }

protected:
    void timerEvent(QTimerEvent *event)
    {
        if (timer_id_1s == event->timerId())
        {
            timeout();
        }
        // else if (timer_id_10ms == event->timerId())
        // {
        // }
    }

private:
    void timeout()
    {
        if (serial_port->open(QIODevice::ReadWrite)) // 用ReadWrite 的模式尝试打开串口
        {
            killTimer(timer_id_1s);
            timer_id_1s = -1;
            usb_connect = true;
            RCLCPP_INFO(nh_->get_logger(), "Connected");
        }
        else
        {
            RCLCPP_INFO(nh_->get_logger(), "trying");
        }
    }

    void errorOccurred(QSerialPort::SerialPortError error)
    {
        if (error == QSerialPort::ResourceError)
        {
            serial_port->close();
            usb_connect = false;
            if (timer_id_1s == -1)
            {
                timer_id_1s = startTimer(1000);
            }
            RCLCPP_INFO(nh_->get_logger(), "Disconnect");
        }
    }

    void readyRead()
    {
        if (serial_port->bytesAvailable())
        {
            QByteArray array = serial_port->readAll();
            // qDebug() << array;
            crsf_array.append(array);
            /* 验证协议 */
            /* 至少有4个 */
        again:
            if (crsf_array.size() > 4)
            {
                int length = (uint8_t)crsf_array[1]; //
                if (length > CRSF_MAX_PACKET_LEN + 4)
                {
                    /* 说明不对 */
                    crsf_array.remove(0, 1);
                    qDebug() << "11";
                    goto again;
                }
                if (crsf_array.size() < (length + 2))
                {
                    return;
                }
                /* 计算crc 结果 */
                if (crc->calc((uint8_t *)(crsf_array.data() + 2), length - 1) != (uint8_t)crsf_array[length + 1])
                {
                    crsf_array.remove(0, 1);
                    qDebug() << "22";
                    goto again;
                }

                /* 协议正确 */
                switch ((uint8_t)crsf_array[0])
                {
                case CRSF_ADDRESS_FLIGHT_CONTROLLER:
                {
                    if (address_flight_controller((uint8_t *)crsf_array.data()) == false)
                    {
                        qDebug() << "Do not process:" << (uint8_t) * (crsf_array.data() + 2);
                    }
                }
                break;

                default:
                    break;
                }

                crsf_array.remove(0, length + 2);
                goto again;
            }
        }
    }

    // bool
    int16_t channels[16] = {0};
    crsfLinkStatistics_t linkStatistics;

private:
    QSerialPort *serial_port = nullptr;
    QByteArray crsf_array; // 读取全部数据
    Crc8 *crc;
    /* 时间  1s   10ms */
    int timer_id_1s = -1;
    // int timer_id_10ms = -1;
    /* 连接状态 */
    bool flag_rc = false;
    int flag_rc_cnt = 0;
    bool rc_connect = false;
    bool usb_connect = false;
    const int joy_mid_value = 1024;        //  中间值
    const int joy_low_value = 300;         //  最小的对比值
    const int joy_high_value = 2048 - 300; //  最大的对比值

    bool joy_start = false;
    bool joy_lock = true; // 防止一上电，车就跑了

    int joy_dir = 1;
    int joy_stop_cnt = 0;
    int joy_stop_max = 30 * 100; // 30s
    int8_t man_nav_mode = 0;     // 0 手动  1 自动  2 上次失联  这次不可以直接连上  需要将自动模式取消  再打开
    /* safe lock */

    bool address_flight_controller(uint8_t *crsfData)
    {
        switch (crsfData[2])
        {
        case CRSF_FRAMETYPE_GPS:
        {
        }
        break;
        case CRSF_FRAMETYPE_BATTERY_SENSOR:
        {
        }
        break;
        case CRSF_FRAMETYPE_LINK_STATISTICS:
        {
            /* 连接状态 */
            crsfLinkStatistics_t *sta = (crsfLinkStatistics_t *)&crsfData[3];
            memcpy(&linkStatistics, sta, sizeof(crsfLinkStatistics_t));
            // qDebug() << "downlink_Link_quality" << linkStatistics.downlink_Link_quality << linkStatistics.downlink_RSSI << linkStatistics.downlink_SNR << linkStatistics.uplink_Link_quality;
            return true;
        }
        break;
        case CRSF_FRAMETYPE_OPENTX_SYNC:
        {
        }
        break;
        case CRSF_FRAMETYPE_RADIO_ID:
        {
        }
        break;
        case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
        {
            int16_t channels[16];
            channels[0] = ((crsfData[3] | crsfData[4] << 8) & 0x07FF);
            channels[1] = ((crsfData[4] >> 3 | crsfData[5] << 5) & 0x07FF);
            channels[2] = ((crsfData[5] >> 6 | crsfData[6] << 2 | crsfData[7] << 10) & 0x07FF);
            channels[3] = ((crsfData[7] >> 1 | crsfData[8] << 7) & 0x07FF);
            channels[4] = ((crsfData[8] >> 4 | crsfData[9] << 4) & 0x07FF);
            channels[5] = ((crsfData[9] >> 7 | crsfData[10] << 1 | crsfData[11] << 9) & 0x07FF);
            channels[6] = ((crsfData[11] >> 2 | crsfData[12] << 6) & 0x07FF);
            channels[7] = ((crsfData[12] >> 5 | crsfData[13] << 3) & 0x07FF); // & the other 8 + 2 channels if you need them
            channels[8] = ((crsfData[14] | crsfData[15] << 8) & 0x07FF);
            channels[9] = ((crsfData[15] >> 3 | crsfData[16] << 5) & 0x07FF);
            channels[10] = ((crsfData[16] >> 6 | crsfData[17] << 2 | crsfData[18] << 10) & 0x07FF);
            channels[11] = ((crsfData[18] >> 1 | crsfData[19] << 7) & 0x07FF);
            channels[12] = ((crsfData[19] >> 4 | crsfData[20] << 4) & 0x07FF);
            channels[13] = ((crsfData[20] >> 7 | crsfData[21] << 1 | crsfData[22] << 9) & 0x07FF);
            channels[14] = ((crsfData[22] >> 2 | crsfData[23] << 6) & 0x07FF);
            channels[15] = ((crsfData[23] >> 5 | crsfData[24] << 3) & 0x07FF);
            memcpy(this->channels, channels, sizeof(this->channels));
            /* 接收到rc的数据 */
            flag_rc = true;
            rc_connect = true;
            // for (int i = 0; i < 26; i++)
            // {
            //     printf("%d ", this->channels[i]);
            // }
            // std::cout << std::endl;

            return true;
        }
        break;
        case CRSF_FRAMETYPE_ATTITUDE:
        {
        }
        break;
        case CRSF_FRAMETYPE_FLIGHT_MODE:
        {
        }
        break;
        case CRSF_FRAMETYPE_DEVICE_PING:
        {
        }
        break;
        case CRSF_FRAMETYPE_DEVICE_INFO:
        {
        }
        break;
        case CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY:
        {
        }
        break;
        case CRSF_FRAMETYPE_PARAMETER_READ:
        {
        }
        break;
        case CRSF_FRAMETYPE_PARAMETER_WRITE:
        {
        }
        break;
        case CRSF_FRAMETYPE_COMMAND:
        {
        }
        break;

        case CRSF_FRAMETYPE_MSP_REQ:
        {
        }
        break;

        case CRSF_FRAMETYPE_MSP_RESP:
        {
        }
        break;
        case CRSF_FRAMETYPE_MSP_WRITE:
        {
        }
        break;

        default:
        {
        }
        break;
        }
        return false;
    }
};

void gquit(int sig)
{
    (void)sig;
    rclcpp::shutdown();
    QCoreApplication::exit(0);
}

class ADCApp : public QCoreApplication
{
public:
    rclcpp::Node::SharedPtr nh_;
    explicit ADCApp(int &argc, char **argv)
        : QCoreApplication(argc, argv)
    {
        rclcpp::init(argc, argv);
        nh_ = rclcpp::Node::make_shared("crsf_joystick_publisher");
        signal(SIGINT, gquit);
    }

    ~ADCApp()
    {
        rclcpp::shutdown();
    }

    int exec()
    {
        AdcJson adc(nh_);
        adc.new_thread_spin();
        return QCoreApplication::exec();
    }
};

int main(int argc, char *argv[])
{
    ADCApp app(argc, argv);
    return app.exec();
}
